#ifndef PROJECT_PID_PATH_FOLLOW_H
#define PROJECT_PID_PATH_FOLLOW_H

#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <std_msgs/String.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf/tf.h>

namespace pid_path_follow{

    class LinearPID{
    public:
        LinearPID()
        {
            linear_err = 0.0;
            linear_err_last = 0.0;
            linear_err_l_last = 0.0;
            actual_linear_vel = 0.0;

        }
        double linear_err;   //偏差
        double linear_err_last; //上次偏差
        double linear_err_l_last; //上上次偏差
        double Kp,Ki,Kd;
        double actual_linear_vel;  //实际速度
        double set_vel = 0.0;  //设定速度

    };

    class AngularPID{
    public:
        AngularPID()
        {
            angular_err = 0.0;
            angualr_err_last = 0.0;
            integral = 0.0;
            actual_angular_vel = 0.0;
        }
        double angular_err ;  //偏差
        double angualr_err_last ;  //上次偏差
        double integral ;   //误差累计
        double Kp, Ki, Kd;
        double actual_angular_vel ; //实际速度
        double set_angualr_vel = 0.0; //设定速度

    };

    class Control{
    public:
        Control();

    private:
        void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& amcl_pose);
        void robotPathCallBack(const nav_msgs::PathConstPtr& path_msg);
        void robotVelCallBack(const geometry_msgs::Twist::ConstPtr& vel_msg);
        
        void calculate_vel();
        void update_point();
        double calculate_angular();                     

        double linearPidControl(double goal_vel, LinearPID &linear);
        double angularPidControl(double angular_vel, AngularPID &angular);

        void controlLoopCB(const ros::TimerEvent& );

        inline double distance(double x1, double y1, double x2, double y2)
        {
            return std::sqrt((x1-x2)* (x1-x2) + (y1 - y2) * (y1 -y2));
        }

    private:
        ros::NodeHandle nh_;
        ros::Publisher cmd_vel_pub_;
        ros::Publisher path_end_point_;
        ros::Subscriber amcl_pose_sub_;
        ros::Subscriber path_sub_;
        ros::Subscriber cmd_vel_sub_;
        
        ros::Timer timer_;
        double t_ ;
        int point_i_, controller_freq_;
        geometry_msgs::Pose current_pose_, goal_pose_, end_pose_;
        nav_msgs::Path nav_path_;
        geometry_msgs::Twist cmd_vel_;
        LinearPID linear_vel_;
        AngularPID angular_vel_;
        bool recv_path_flag_ , nav_flag_;
        double want_vel_;
	bool loop_;

    };
}


#endif //PROJECT_PID_PATH_FOLLOW_H
